#! /usr/bin/env python

PACKAGE = 'velocity_controller'

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator ()

# closest waypoint threshold
gen.add ("closest_waypoint_distance_threshold", double_t, 0, "closest waypoint distance threshold", 3.0, 0.0, 10.0)
gen.add ("closest_waypoint_angle_threshold", double_t, 0, "closest waypoint angle threshold", 0.7854, 0.0, 2.0)

# stop state
gen.add ("stop_state_velocity", double_t, 0, "stop state velocity[m/s]", 0.0, 0.0, 1.0)
gen.add ("stop_state_acc", double_t, 0, "stop state acc[m/s^2]", -3.4, -6.0, 6.0)
gen.add ("stop_state_entry_ego_speed", double_t, 0, "stop state entry ego speed[m/s]", 0.2, 0.0, 1.0)
gen.add ("stop_state_entry_target_speed", double_t, 0, "stop state entry target speed[m/s]", 0.1, 0.0, 1.0)

# delay compensation
gen.add ("delay_compensation_time", double_t, 0, "delay compensation[s]", 0.17, 0.0, 2.0)

# emergency stop by this controller
gen.add ("emergency_stop_acc", double_t, 0, "emergency stop acc[m/s^2]", -5.0, -10.0, 0.0)
gen.add ("emergency_stop_jerk", double_t, 0, "emergency stop jerk[m/s^3]", -3.0, -10.0, 0.0)

# smooth stop
gen.add ("exit_ego_speed", double_t, 0, "exit ego speed[m/s]", 1.0, 0.0, 5.0)
gen.add ("entry_ego_speed", double_t, 0, "entry ego speed[m/s]", 0.8, 0.0, 2.0)
gen.add ("exit_target_speed", double_t, 0, "exit target speed[m/s]", 1.0, 0.0, 5.0)
gen.add ("entry_target_speed", double_t, 0, "entry target speed[m/s]", 0.2, 0.0, 1.0)
gen.add ("weak_brake_time", double_t, 0, "weak brake time[s]", 1.0, 0.0, 2.0)
gen.add ("weak_brake_acc", double_t, 0, "weak brake acc[m/s^2]", -1.0, -5.0, 0.0)
gen.add ("increasing_brake_time", double_t, 0, "increasing brake time[s]", 1.0, 0.0, 5.0)
gen.add ("increasing_brake_gradient", double_t, 0, "increasing brake gradient[m/s^3]", -0.1, -1.0, 0.0)
gen.add ("stop_brake_time", double_t, 0, "stop brake time[s]", 1.0, 0.0, 5.0)
gen.add ("stop_brake_acc", double_t, 0, "stop brake acc[m/s^2]", -3.4, -5.0, 0.0)
gen.add ("stop_state_keep_stopping_dist", double_t, 0, "keep stopping distance [m]", 0.5, 0.0, 4.0)



# acceleration limit
gen.add ("max_acc", double_t, 0, "max acc[m/s^2]", 3.0, 0.1, 5.0)
gen.add ("min_acc", double_t, 0, "min acc[m/s^2]", -5.0, -10.0, -0.1)

# jerk limit
gen.add ("max_jerk", double_t, 0, "max jerk[m/s^3]", 2.0, 0.1, 5.0)
gen.add ("min_jerk", double_t, 0, "min jerk[m/s^3]", -5.0, -10.0, -0.1)

# slope compensation
gen.add ("max_pitch_rad", double_t, 0, "max pitch rad[rad]", 0.1, 0.0, 1.0)
gen.add ("min_pitch_rad", double_t, 0, "min pitch rad[rad]", -0.1, -1.0, 0.0)
gen.add ("lpf_pitch_gain", double_t, 0, "lpf pitch gain", 0.95, 0.0, 2.0)

# velocity feedback
gen.add ("kp", double_t, 0, "Kp", 1.0, 0.0, 5.0)
gen.add ("ki", double_t, 0, "Ki", 0.1, 0.0, 5.0)
gen.add ("kd", double_t, 0, "Kd", 0.0, 0.0, 5.0)
gen.add ("max_out", double_t, 0, "max out[m/s^2]", 1.0, 0.0, 5.0)
gen.add ("min_out", double_t, 0, "min out[m/s^2]", -1.0, -5.0, 0.0)
gen.add ("max_p_effort", double_t, 0, "max_p_effort[m/s^2]", 1.0, 0.1, 5.0)
gen.add ("min_p_effort", double_t, 0, "min_p_effort[m/s^2]", -1.0, -5.0, 0.0)
gen.add ("max_i_effort", double_t, 0, "max_i_effort[m/s^2]", 0.3, 0.0, 1.0)
gen.add ("min_i_effort", double_t, 0, "min_i_effort[m/s^2]", -0.3, -1.0, 0.0)
gen.add ("max_d_effort", double_t, 0, "max_d_effort[m/s^2]", 0.0, 0.0, 5.0)
gen.add ("min_d_effort", double_t, 0, "min_d_effort[m/s^2]", 0.0, 0.0, 5.0)
gen.add ("current_velocity_threshold_pid_integration", double_t, 0, "current_velocity_threshold_pid_integration[m/s]", 0.5, 0.0, 2.0)
gen.add ("lpf_velocity_error_gain", double_t, 0, "lpf_velocity_error_gain", 0.9, 0.0, 3.0)

exit (gen.generate (PACKAGE, "velocity_controller", "VelocityController"))
